/* --------------------------------------------------------------------------
 * GraphomatJr - RoboServoController
 * --------------------------------------------------------------------------
 * prog:  Max Rheiner / Interaction Design / Zhdk / http://iad.zhdk.ch/
 * date:  08/26/2012 (m/d/y)
 * ----------------------------------------------------------------------------
 *
 * ----------------------------------------------------------------------------
 */

import java.lang.Thread;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.ListIterator;
import java.util.ListIterator;
import java.io.File;
import java.io.FileReader;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.PrintWriter;
import java.lang.reflect.Method;

import processing.core.*;
import SimpleDynamixel.*;
import SimpleKDL.*;

// inkJr --> luxo jr
public class RoboServoController extends Thread
{
  public final static int		ROBO_MODE_DEFAULT				= 0;
  public final static int		ROBO_MODE_CAPTURE				= 1;

  public final static int		ROBO_STATE_NO					= 0;
  public final static int		ROBO_STATE_DRAW_PRE_POSITION	= 1;
  public final static int		ROBO_STATE_DRAW_PRE_POSITION_END= 2;
  public final static int		ROBO_STATE_DRAW_DOWN			= 3;
  public final static int		ROBO_STATE_DRAW_DOWN_END		= 4;
  public final static int		ROBO_STATE_DRAW_UP				= 5;
  public final static int		ROBO_STATE_DRAW_UP_END			= 6;
  public final static int		ROBO_STATE_DRAWING_START		= 7;
  public final static int		ROBO_STATE_DRAWING				= 8;
  public final static int		ROBO_STATE_DRAWING_END			= 9;
  public final static int		ROBO_STATE_START				= 10;
  public final static int		ROBO_STATE_END					= 11;
  public final static int		ROBO_STATE_PAUSE				= 12;

  public final static int		ROBO_MOTOR_STATE_NO				= 0;
  public final static int		ROBO_MOTOR_STATE_MOVE			= 1;
  public final static int		ROBO_MOTOR_STATE_MOVE_END		= 2;
 
  public final static int		ROBO_VIZ_DEFAULT				= 0;
  public final static int		ROBO_VIZ_INFO					= 1 << 0;


  protected ArrayList<ArrayList<PVector>> 		_polygonList = null;
  protected int									_curPolygonListIndex;
  protected ArrayList<PVector>			 		_curPolygon = null;
  protected int									_curPolygonIndex;

  protected long								_curPolygonTotalTime;
  protected long								_curPolygonStartTime;
  protected long								_curPolygonTime;
  protected float								_curPolygonNormTime;
  protected long								_polygonPauseTime;
  protected float								_polygonDrawSpeed;		// mm / time
  protected float								_positioningSpeed;		// mm / time
  protected float								_positioningUpSpeed = 0;
  protected float								_positioningDownSpeed = 0;

  protected PVector 							_minVec = new PVector();
  protected PVector 							_maxVec = new PVector();
  protected PVector 							_polygonOffsetTrans = new PVector();

  protected ArrayList<PVector>			 		_curOutPoly = null;
  protected int									_curOutPolyIndex = 0;

  protected int									_threadSleepTime = 1;

  protected KinematicSolver					_kinematicSolver;
  protected Servo							_servo;		

  protected int[]							_servoIds;
  protected int[]							_servoModels;
  protected float[]							_servoAngleOffset;
  protected float[]							_servoAngleRest;

  protected float							_angle2Pos;
  protected float							_pos2Angle;
  protected int								_servoRange = 0xFFF; 	// 12bit dynamixel mx
  protected float							_servoDeadAngle = 0; 	// in rad
  protected float							_minActionRadius;
  protected float							_maxActionRadius;

  protected PVector							_curPos = new PVector();
  protected PVector							_curNormal = new PVector(0,0,1);
  protected float							_penUpDist = 50;		// 50mm
  protected PVector							_penUpVec;
  protected PVector							_curPolyStartPos = null; 
  protected PVector							_curPolyStartEnd = null; 

  protected	Object							_parent;
  protected int								_robotState 	= ROBO_STATE_NO;
  protected int								_robotStateOld 	= ROBO_STATE_NO;
  protected int								_robotMotorState = ROBO_MOTOR_STATE_NO;
  protected int								_robotMode = ROBO_MODE_DEFAULT;

  protected float							_resampleResolution 		= 0.005f;	// in mm
  protected float							_resampleResolutionFast 	= 2.0f;	// in mm

  protected int								_speedDrawPos = 0;
  protected int								_speedPreDrawing = 0;
  protected int								_speedDrawing = 0;

  protected Object							_lock = new Object();

  protected ArrayList<PVector>				_scanPosList;
  protected int[]							_lastGoalPositions = null;
  protected PMatrix3D						_baseXform = new PMatrix3D();
  protected PMatrix3D						_invBaseXform = new PMatrix3D();
  protected PMatrix3D						_baseXformNormal = new PMatrix3D();
  protected boolean							_calibXformFlag = false;
  protected PVector							_pCalibCenter;
  protected PVector							_pCalibX;
  protected PVector							_pCalibY;
  protected PVector							_pCalibZ;

  protected PVector _planeP = new PVector(0, 0, 0);
  protected PVector _planeX = new PVector(1, 0, 0);
  protected PVector _planeY = new PVector(0, 1, 0);

  protected boolean 						_enable = true;
  protected boolean							_motorEnable = true;

  ServoViz[]								_servoVizList;
  int										_vizFlags = ROBO_VIZ_DEFAULT;

  protected Method            				_statusCBMethod = null;
  protected Object            				_statusCBObject = null;

  public RoboServoController(Object 				parent,	
							 SimpleDynamixel.Servo 	servo,
							 KinematicSolver		kinematicSolver,
							 int[]					servoIds,			// bottom - up
							 float[]				servoAngleOffset,
							 float					minActionRadius,
							 float					maxActionRadius)
  {
	_parent = parent;

	_servo = servo;
	_kinematicSolver = kinematicSolver;

	// set callback for custom drawing
	_kinematicSolver.setCallback(this);
	// set drawing scale for coordsys viz
	_kinematicSolver.setScale(0.5f);
	
	// status callback
	setCallback(parent);

	_servoIds = servoIds;
	_servoAngleOffset = servoAngleOffset;
	_lastGoalPositions = new int[_servoIds.length];

	_angle2Pos = (float)_servoRange / (PConstants.TWO_PI - _servoDeadAngle) + (_servoDeadAngle * .5f);
	_pos2Angle = (PConstants.TWO_PI - _servoDeadAngle) / (float)_servoRange;

	_minActionRadius = minActionRadius;
	_maxActionRadius = maxActionRadius;

	_polygonList = null;

	_curPolygonStartTime = 0;
	_curPolygonTime = 0;
	_curPolygonNormTime = 0;
	_curPolygonTotalTime = 0;
	_polygonDrawSpeed = 40;	// mm per second	
	_positioningSpeed = 300;
	_positioningUpSpeed = 150;
	_positioningDownSpeed = 40;

	_penUpVec = PVector.mult(_curNormal,_penUpDist);

	_servoAngleRest = _kinematicSolver.getOutAngles() ;
	
	// test if the motors are online
	int[]   servoList = servo.pingRange(0,10);
	if(servoList.length == 0)
	{
	  _motorEnable = false;
	  System.out.println("RoboServoController: no motors found!");
	}
	else
	{	// read out all servo models
		_servoModels = new int[_servoIds.length];
		for(int i=0;i < _servoIds.length;i++)
		{
		  _servoModels[i] = _servo.modelNr(_servoIds[0]);
		}
	}
  
	// init the motors
	initMotors();
	moveRestPos();

	if(_motorEnable)
	{
	  // init all servos
	  // set the delay time of the status packet to 0, have enough big rx buffer
	  _servoVizList = new ServoViz[_servoIds.length];
	  for(int i=0;i < _servoIds.length;i++)
		_servoVizList[i] = new ServoViz(servo,_servoIds[i],_servoRange,(int)_servoDeadAngle);
	}
  }

  public void setPenUpDist(float dist)
  {
	_penUpDist = dist;
	_penUpVec = PVector.mult(_curNormal,_penUpDist);
  }

  public void setCallback(Object listener)
  {
      _statusCBMethod = KinematicSolver.getMethodRef(listener,"onRoboStatus",new Class[] { RoboServoController.class, int.class });
      _statusCBObject = listener;
  }
  
  protected void callCB()
  {
	if(_statusCBMethod == null)
	  return;
            
	try
	{
	  _statusCBMethod.invoke(_statusCBObject,
							 new Object[] { this,
											_robotState,
										  }
							);
	}
    catch (Exception e)
    {}
  }

  public SimpleKDL.KinematicSolver kinematicSolver() { return _kinematicSolver; }

  public void initMotors()
  {
	if(_motorEnable == false)
	  return;

	float[]	minAngles = _kinematicSolver.getMinAngles();
	float[]	maxAngles = _kinematicSolver.getMaxAngles();
	int minPos;
	int maxPos;

	for(int i=0;i < _servoIds.length;i++)
	{
	  _servo.setDelayTime(_servoIds[i],0);
	  //servo.setDelayTime(_servoIds[i],0xFA);

	  _servo.setPunch(_servoIds[i],0);
/*
	  servo.setPGain(_servoIds[i],32);
	  servo.setIGain(_servoIds[i],0);
	  servo.setDGain(_servoIds[i],0);
*/	
	  // set the limits for the angles per motor
	  minPos = calcServoPos(_servoAngleOffset[i],minAngles[i],_servoModels[i]);
	  maxPos = calcServoPos(_servoAngleOffset[i],maxAngles[i],_servoModels[i]);
	  
	  _servo.setAngleLimitCW(_servoIds[i], maxPos);
	  _servo.setAngleLimitCCW(_servoIds[i],minPos);
	}	
  }

  public void moveRestPos()
  {
	_kinematicSolver.setOutAngles(KinematicSolver.toJntArray(_servoAngleRest));
	if(_motorEnable)
	  _servo.syncWriteMovingSpeed(_servoIds,100);
	setCurPosition(true);
  }
  
  public Servo servo()
  {
	return _servo;
  }

  public boolean setPolygonList(ArrayList<ArrayList<PVector>>  polygonList)
  {
	if(polygonList.size() <= 0)
	  return false;

	_polygonList = polygonList;
	//_polygonList = PathReader.resamplePolygonList(polygonList,0.01f);
	_curPolygonListIndex = 0;

	// calc min manx
	PathReader.calcMinMax(_polygonList,_minVec,_maxVec);
	// center the drawing
	_polygonOffsetTrans = PVector.mult(PVector.sub(_maxVec,_minVec),.5f);
	return true;
  }

  public int state() 
  {
	int ret;
	synchronized(_lock)
	{
	  ret = _robotState;
	}
	return ret;
  }

  public PVector getEndPos()
  {
	synchronized(_lock)
	{
	  // define path
	  PMatrix3D	mat = KinematicSolver.getMatrix(_kinematicSolver.solveFk());
	  PVector startPoint = new PVector(); 
	  mat.mult(new PVector(0,0,0),startPoint);
	  
	  if(_calibXformFlag)
	  {
		PVector endPoint = new PVector();
		_invBaseXform.mult(startPoint,endPoint);
		return endPoint;	
	  }
	  else
		return startPoint;
	}
  }


  public boolean startDrawing()
  {
	synchronized(_lock)
	{
	  if(_polygonList == null)
		return false;
	  
	  _curPolygonListIndex = 0;

	  if(setNextPolygon() == false)
		return false;

	  // start to move to the first pos
	  setState(ROBO_STATE_START);

	  // define path
	  PMatrix3D	mat = KinematicSolver.getMatrix(_kinematicSolver.solveFk());
	  PVector startPoint = new PVector(); 
	  mat.mult(new PVector(0,0,0),startPoint);
	  
	  if(_calibXformFlag)
	  {
		PVector endPoint = new PVector();
		_invBaseXform.mult(startPoint,endPoint);
		startPoint = endPoint;		
	  }

	  setMovingSpeed(_speedDrawPos);
	  setCurOutPoly(startPoint,_curPolyStartPos,_resampleResolutionFast,_positioningSpeed);
	}

	return true;
  }

  public boolean stopDrawing()
  {
	synchronized(_lock)
	{
	  // start to move to the first pos
	  setState(ROBO_STATE_END);
	  moveRestPos();
	}
	return true;
  }

  public boolean pauseDrawing()
  {
	synchronized(_lock)
	{
	  // start to move to the first pos
	  _robotStateOld = _robotState;
	  _polygonPauseTime = System.currentTimeMillis();
	  setState(ROBO_STATE_PAUSE);
	}
	return true;
  }

  public boolean playDrawing()
  {
	synchronized(_lock)
	{
	  if(_robotState == ROBO_STATE_PAUSE)
	  {
		_curPolygonStartTime += System.currentTimeMillis() - _polygonPauseTime;
		setState(_robotStateOld);
		return true;
	  }
	}
	return false;
  }

  protected boolean setNextPolygon()
  {
	if(_polygonList == null)
	  return false;

	if(_curPolygonListIndex >= _polygonList.size())
	{	// reached end
	  return false;
	}

	_curPolygon = PathReader.resamplePolygon(_polygonList.get(_curPolygonListIndex),_resampleResolution);
	_curPolygonIndex = 0;

	_curPolygonListIndex++;

	// set start end position for the polygon
	_curPolyStartPos = PVector.add(_curPolygon.get(0),_penUpVec);
	_curPolyStartEnd = PVector.add(_curPolygon.get(_curPolygon.size() -1),_penUpVec);
	//_curPolyStartEnd = PVector.add(_curPolygon[_curPolygon.length -1], PVector.mult(_penUpVec,-1.0f));
	
	//System.out.println("setNextPolygon: " + _curPolygon.size());

	return true;
  }

  protected void setCurOutPoly(PVector p1,PVector p2,float resampleDist, float speed)
  {
	ArrayList<PVector> poly = new ArrayList<PVector>();
	poly.add(p1);
	poly.add(p2);

	setCurOutPoly(poly,resampleDist,speed);
  }

  protected void setCurOutPoly( ArrayList<PVector> curOutPoly, float resampleDist,float speed)
  {
	_curOutPoly = PathReader.resamplePolygon(curOutPoly,resampleDist);
	_curOutPolyIndex = 0;

	resetPolygonTimer(PathReader.getPolygonLength(_curOutPoly),speed);
  }

  protected boolean updateCurOutPoly()
  {
	if(_curOutPolyIndex == 0)
	{	// start
	  resetPolygonTimer();

	  moveTo(_curOutPoly.get(_curOutPolyIndex),_curNormal);	
	  _curOutPolyIndex++;
	}
	else
	{	// set cur pos
	  updatePolygonTimer();

	  //if(_curPolygonNormTime  > 1.0f)
	  if(_curPolygonNormTime  > 1.001f)
	  {	// finished
		return false;
	  }
	  else
	  {	
		PVector curPos = PathReader.getInterpPos(_curOutPoly,_curPolygonNormTime);
		moveTo(curPos,_curNormal);	
	  }
	}
	return true;

/*
	if(_curOutPolyIndex == 0)
	{	// start
	  moveTo(_curOutPoly.get(_curOutPolyIndex),_curNormal);	
	  _curOutPolyIndex++;
	}
	else if(isMoving() == false)
	{	// done with the moving, go to next pos
	  if(_curOutPolyIndex >= _curOutPoly.size())
	  {	// finished
		return false;
	  }
	  else
	  {	
		moveTo(_curOutPoly.get(_curOutPolyIndex),_curNormal);	
		_curOutPolyIndex++;
	  }
	}
	return true;
*/
  }

  protected void setState(int state)
  {
	if(state == _robotState)
	  return;

	_robotState = state;
	callCB();
  }

  public void update()
  {

	switch(_robotState)
	{
	case ROBO_STATE_START:
	  if(updateCurOutPoly() == false)
	  {
		setState(ROBO_STATE_DRAW_DOWN);
	  }
	  break;
	case ROBO_STATE_DRAW_PRE_POSITION:
	  if(updateCurOutPoly() == false)
	  {
		setCurOutPoly(_curOutPoly.get(_curOutPoly.size()-1).get(),
					  _curPolyStartPos,
					  _resampleResolutionFast,
					  _positioningSpeed);

		setMovingSpeed(_speedDrawPos);
		setState(ROBO_STATE_DRAW_PRE_POSITION_END);
	  }
	  break;
	case ROBO_STATE_DRAW_PRE_POSITION_END:
	  if(updateCurOutPoly() == false)
	  {
		setState(ROBO_STATE_DRAW_DOWN);
	  }
	  break;

	case ROBO_STATE_DRAW_DOWN:
	  setCurOutPoly(_curOutPoly.get(_curOutPoly.size()-1).get(),
					_curPolygon.get(0),
					_resampleResolutionFast,
					_positioningDownSpeed);
	  setMovingSpeed(_speedPreDrawing);
	  setState(ROBO_STATE_DRAW_DOWN_END);
	  break;
	case ROBO_STATE_DRAW_DOWN_END:
	  if(updateCurOutPoly() == false)
	  {
		setState(ROBO_STATE_DRAWING_START);
	  }
	  break;

	case ROBO_STATE_DRAWING_START:
	  // robot actually draws
	  setMovingSpeed(_speedDrawing);
	  setCurOutPoly(_curPolygon,
					_resampleResolution,
					_polygonDrawSpeed);
	  setState(ROBO_STATE_DRAWING);
	  break;
	case ROBO_STATE_DRAWING:
	  // robot actually draws
	  if(updateCurOutPoly() == false)
	  {
		setState(ROBO_STATE_DRAWING_END);
	  }
	  break;
	case ROBO_STATE_DRAWING_END:
	  setCurOutPoly(_curOutPoly.get(_curOutPoly.size()-1).get(),
					_curPolyStartEnd,
					_resampleResolutionFast,
					_positioningUpSpeed);			
	  setState(ROBO_STATE_DRAW_UP);
	  break;

	case ROBO_STATE_DRAW_UP:
	  // move the robot to save pos
	  if(updateCurOutPoly() == false)
	  {
		if(setNextPolygon())
		{	// go to the next start pos
		  setState(ROBO_STATE_DRAW_PRE_POSITION);
		}
		else
		{	// done
		  setState(ROBO_STATE_END);
		  moveRestPos();
		}
	  }
	  break;
	}
  }

  public void run()
  {
	while(true)
	{
	  synchronized(_lock)
	  {
		if(_enable)
		{
		  switch(_robotMode)
		  {
		  case ROBO_MODE_CAPTURE:
			setCurServoAngles();
			break;
		  case ROBO_MODE_DEFAULT:
		  default:
			//long startTime = System.currentTimeMillis();	
			update();
			//System.out.println("xxx readtime:" + (System.currentTimeMillis()- startTime));
			break;
		  }
		}
  
		// update the viz
		if((_vizFlags & ROBO_VIZ_INFO) > 0 && _servoVizList != null)
		{
		  for(int i=0;i < _servoVizList.length;i++)
			_servoVizList[i].update();
		}	
	  }

      try
      {
		Thread.sleep(_threadSleepTime);
      }
      catch(InterruptedException excetpion)
      {
		System.out.println("InterruptedException: " + excetpion);
		return;
      }	
	}
  }

  public void setMovingSpeed(int speed)
  {
	if(_motorEnable == false)
	  return;
	/*
	_servo.beginRegWrite();
	  for(int i=0;i<_servoIds.length;i++)
		_servo.setMovingSpeed(_servoIds[i],speed);	  
	_servo.endRegWrite();
	*/
	_servo.syncWriteMovingSpeed(_servoIds,speed);
  }

  public void setCurPosition(int speed)
  {
	if(_motorEnable)
	  _servo.syncWriteMovingSpeed(_servoIds,speed);
	setCurPosition();
  }

  public void setCurPosition()
  {
	setCurPosition(false);
  }

  public void setCurPosition(boolean force)
  {
	if(_motorEnable == false)
	  return;

	float[] outAngles = _kinematicSolver.getOutAngles();

//	System.out.println("------------");
//	for(int i=0;i<_servoIds.length;i++)
//	  System.out.println("motor:" + _servoIds[i] + ",\tangle:" + PApplet.degrees(outAngles[i]) + ",\tpos:" + calcServoPos(_servoAngleOffset[i],outAngles[i]) );


	int[] posList = new int[_servoIds.length];
	for(int i=0;i<_servoIds.length;i++)
	  posList[i] = calcServoPos(_servoAngleOffset[i],outAngles[i],_servoModels[i]);

	if(force)
	  _servo.syncWriteGoalPosition(_servoIds,posList);
	else
	{
	  if(checkLastGoalPos(posList))
	  {
		_servo.syncWriteGoalPosition(_servoIds,posList);
		_lastGoalPositions = posList.clone();
	  }
	}
  }

  protected boolean checkLastGoalPos(int[] newGoalPos)
  {
	for(int i=0;i < newGoalPos.length;i++)
	{
	  if(_lastGoalPositions[i] != newGoalPos[i])
		return true;
	}
	return false;
  }

  public static float angle2Pos(int servoModel)
  {
	float range 	= (float)Servo.getMotorRange(servoModel);	
	float deadAngle = (float)Math.toRadians(Servo.getMotorDeadAngle(servoModel));

	return (range / (PConstants.TWO_PI - deadAngle) + (deadAngle * .5f));
  }

  public static float pos2Angle(int servoModel)
  {
	float range	 	= (float)Servo.getMotorRange(servoModel);	
	float deadAngle = (float) Math.toRadians(Servo.getMotorDeadAngle(servoModel));

	return ((PConstants.TWO_PI - deadAngle) / (float)range);
  }

  public int calcServoPos(float offset,float angle,int servoModel)
  {
	float endAngle = offset - angle;
	if(endAngle < 0.0f)
	  endAngle = PConstants.TWO_PI - (PApplet.abs(endAngle) % PConstants.TWO_PI);
	else
	  endAngle = endAngle % PConstants.TWO_PI;
	  
	//return((int)(endAngle * _angle2Pos)); 
	return((int)(endAngle * angle2Pos(servoModel))); 
  }

  public float calcServoAngle(float offset,int pos,int servoModel)
  {
	float angle = (pos2Angle(servoModel) * (float)(_servoRange - pos) + offset);
	//float angle = (_pos2Angle * (float)(_servoRange - pos) + offset);
	if(angle < 0.0f)
	  angle = PConstants.TWO_PI - (PApplet.abs(angle) % PConstants.TWO_PI);
	else
	  angle = angle % PConstants.TWO_PI;

	return angle;
  }

  public boolean isMoving()
  {
	if(_motorEnable == false)
	  return false;

	for(int i=0;i<_servoIds.length;i++)
	{
	  if(_servo.moving(_servoIds[i]))
		return true;
	}
	return false;
  }

  public boolean definePlane(PVector p1,PVector p2, PVector p3)
  {
/*
    float[] tf=new float[16];

        SimpleKDLMain.getXFormMat(origCenter.array(),
                                  origX.array(),
                                  origY.array(),
                                  origZ.array(),
                                  newCenter.array(),
                                  newX.array(),
                                  newY.array(),
                                  newZ.array(),
                                  tf);
*/
	return false;
  }
  
  public SimpleKDL.Frame calcEndRotMat(PVector start,PVector end, PVector normalEnd)
  {
	PVector v = PVector.sub(end,start);
	PVector n = normalEnd.get();
	v.normalize();
	n.normalize();

	PVector u = v.cross(n);
	u.normalize();

	v = n.cross(u);
	v.normalize();

	// the end frame should be aligned with the chain base
	SimpleKDL.Frame rotFrame = SimpleKDL.KinematicSolver.getFrame(// orig
																new PVector(0,0,0),  // x
																new PVector(1,0,0),  // x
																new PVector(0,1,0),  // y
																new PVector(0,0,1),  // z
																// dest
																new PVector(0,0,0),  // z
																u,
																v,
																n
																);

	// kdl is in the right hand coordinate sys.
	SimpleKDL.Rotation rotFlip =  new SimpleKDL.Rotation(1,0,0,
														0,1,0,
														0,0,-1);
	SimpleKDL.Rotation rotEnd = SimpleKDL.Rotation.mult(rotFrame.getM(),rotFlip);
 
	return(new SimpleKDL.Frame(rotEnd,new SimpleKDL.Vector(end.x, end.y, end.z)));
  }
  
  public boolean setEndPosGlobal(PVector endPos,PVector endPosNormal)
  {
	// calculate rotation frame according to the chain base
	SimpleKDL.Frame endFrame = calcEndRotMat(new PVector(0,0,0),
										     endPos,
											 endPosNormal);
		  
	// set the end position of the chain  
	return _kinematicSolver.solveIk(endFrame); 
  }

  public boolean setEndPos(PVector endPos,PVector endPosNormal)
  {
	PVector pos;
	PVector normal;

	if(_calibXformFlag)
	{
	  pos = new PVector();
	  normal = new PVector();

	  // calc the new pos
	  _baseXform.mult(endPos,pos);

	  // calculate only the rotation, it's the normal
	  _baseXformNormal.mult(endPosNormal,normal);
	}
	else
	{
	  pos = endPos;
	  normal = endPosNormal;
	}

	// calculate rotation frame according to the chain base
	SimpleKDL.Frame endFrame = calcEndRotMat(new PVector(0,0,0),
										     pos,
											 normal);
	
	// calculate matrix for visualization
	//endPointMat = SimpleKDL.KinematicSolver.getMatrix(endFrame);
	  
	// set the end position of the chain  
	return _kinematicSolver.solveIk(endFrame);  
  }  


  // goes up, to moves
  protected void moveTo(PVector p1,PVector normal,int speed)
  {
	setMovingSpeed(speed);
	moveTo(p1,normal);
  }

  protected void moveTo(PVector p1,PVector normal)
  {
	_robotMotorState = ROBO_MOTOR_STATE_MOVE;

	//	calc the motor angles
	setEndPos(p1,normal);	

	// set the motor angles
	setCurPosition();
  }

  public void startPose()
  {
	synchronized(_lock)
	{
	  if(_motorEnable == false)
		_servo.syncWriteTorqueEnable(_servoIds,false);

	  _robotMode = ROBO_MODE_CAPTURE;
	}
  }

  public void startPlaneScanner()
  {
	synchronized(_lock)
	{
	  if(_motorEnable == false)
		_servo.syncWriteTorqueEnable(_servoIds,false);

	  _scanPosList = new ArrayList<PVector>();

	  _robotMode = ROBO_MODE_CAPTURE;
	}
  }

  public void capturePlaneScanner()
  {
	synchronized(_lock)
	{
	  if(_robotMode != ROBO_MODE_CAPTURE)
		return;

	  // read out the angles and set the forward kinematic solver
	  setCurServoAngles();
	  //_scanPosList.add();

	  PMatrix3D	mat = KinematicSolver.getMatrix(_kinematicSolver.solveFk());
	  PVector capPoint = new PVector(); 
	  mat.mult(new PVector(0,0,0),capPoint);

	  _scanPosList.add(capPoint);
	}
  }

  protected boolean setCurServoAngles()
  {
	  if(_motorEnable == false)
		return false;

	  //System.out.println("---------------");
	  int pos;
	  float[] newOutPos = new float[_servoIds.length]; 
	  for(int i=0;i<_servoIds.length;i++)
	  {
		pos = _servo.presentPosition(_servoIds[i]);
		if(pos < 0)
		  return false;
		newOutPos[i] = calcServoAngle(_servoAngleOffset[i],pos,_servoModels[i]);

//		if(i==0)
//		  System.out.println("calcServoAngle: " + PApplet.degrees(newOutPos[i]) + "\t:" + pos);
	  }
	  _kinematicSolver.setOutAngles(KinematicSolver.toJntArray(newOutPos));

	  return true;
  }

  public boolean endPlaneScanner()
  {
	synchronized(_lock)
	{
	  if(_robotMode != ROBO_MODE_CAPTURE)
		return false;

	  _robotMode = ROBO_MODE_DEFAULT;

	  if(_motorEnable)
		_servo.syncWriteTorqueEnable(_servoIds,true);
	  
	  // sleep for 2 seconds, then move to start pos
      try
      {
		Thread.sleep(2000);
      }
      catch(InterruptedException excetpion)
      {
      }	
	  moveRestPos();

	  // calculate the plane
	  if(_scanPosList != null && _scanPosList.size() >= 3)
	  {		  
		/*
		System.out.println("calbiration points:");
		System.out.println("center" + _scanPosList.get(0));
		System.out.println("xPos" + _scanPosList.get(1));
		System.out.println("yPos" + _scanPosList.get(2));

		return calibrate(_scanPosList.get(0),_scanPosList.get(1),_scanPosList.get(2));
		*/
		return true;
	  }

	  return false;
	}
  }

  public boolean saveCalibrationPoints(String calibFile)
  {
	if(_scanPosList != null && _scanPosList.size() >= 3)
	{
	  File file = new File(calibFile);
	  PrintWriter out;
	  try{
		out = new PrintWriter(file);
	  }catch(IOException e)
	  {
		return false;
	  }
	  
	  PVector p = _scanPosList.get(0);
	  out.println(p.x + "," + p.y + "," + p.z);

	  p = _scanPosList.get(1);
	  out.println(p.x + "," + p.y + "," + p.z);

	  p = _scanPosList.get(2);
	  out.println(p.x + "," + p.y + "," + p.z);

	  out.flush();
	  out.close();
	  return true;
	}
	return false;
  }

  public boolean loadCalibrationPoints(String calibFile)
  {
	try {
	  File file = new File(calibFile);
	  FileReader fileReader = new FileReader(file);
	  BufferedReader bufferedReader = new BufferedReader(fileReader);
	  ArrayList<String> strList = new ArrayList<String>();
	  String line;

	  while(true) 
	  {
		line = bufferedReader.readLine();
		if(line != null)
		  strList.add(line);
		else
		  break;
	  }
	  fileReader.close();
	  
	  if(strList.size() >= 3)
	  {
		PVector[] pList = new PVector[3];
		for (int i =0 ; i < pList.length; i++) 
		{
		  String[] tokens = strList.get(i).split("[,]");
		  if(tokens.length >= 3)
			pList[i] = new PVector(Float.parseFloat(tokens[0]),Float.parseFloat(tokens[1]),Float.parseFloat(tokens[2]));
		  else
			return false;
		}

		return calibrate(pList[0],pList[1],pList[2]);
	  }
	} 
	catch (IOException e) 
	{
	  return false;
	}

	return false;
/*

	String lines[] = loadStrings(calibFile);
	List<String> lines = Files.readLines(file, Charsets.UTF_8);

	if(lines.length >= 3)
	{
	  PVector[] pList = new PVector[3];
	  for (int i =0 ; i < pList.length; i++) 
	  {
		String[] tokens = lines[i].split("[,]");
		if(tokens.length >= 3)
		  pList[i] = new PVector(Float.parseFloat(tokens[0]),Float.parseFloat(tokens[1]),Float.parseFloat(tokens[2]));
		else
		  return false;
	  }

	  return calibrate(pList[0],pList[1],pList[2]);
	}
	
	return lines;
*/
  }

  public boolean getCalibratePoints(PVector p,PVector xPos,PVector yPos)
  {
  	  if(_scanPosList != null && _scanPosList.size() >= 3)
	  {
		p.set(_scanPosList.get(0));
		xPos.set(_scanPosList.get(1));
		yPos.set(_scanPosList.get(2));
		return true;
	  }
	  
	  return false;
  }

  public boolean calibrate(PVector p,PVector xPos,PVector yPos)
  {	
	// calc the transformation mat
		  PVector u = PVector.sub(xPos, p);
		  u.normalize();
		  PVector v = PVector.sub(yPos, p);
		  v.normalize();
		  PVector dirUp = u.cross(v);
		  dirUp.normalize();

		  v = dirUp.cross(u);
		  v.normalize();

		  PVector pX = PVector.add(u,p);
		  PVector pY = PVector.add(v,p);
		  PVector pZ = PVector.add(dirUp,p);

		  _baseXform = KinematicSolver.getXFormMat(new PVector(0,0,0),
												   new PVector(1,0,0),
												   new PVector(0,1,0),
												   new PVector(0,0,1),
												   p,
												   pX,
												   pY,
												   pZ);

		  _pCalibCenter = p;
/*
		  _pCalibX = PVector.mult(u,PVector.sub(xPos, p).mag());
		  _pCalibY = PVector.mult(u,PVector.sub(yPos, p).mag());
		  _pCalibZ = PVector.mult(dirUp,PVector.sub(xPos, p).mag());
*/
		  _pCalibX = PVector.add(p,PVector.mult(u,200));
		  _pCalibY = PVector.add(p,PVector.mult(v,200));
		  _pCalibZ = PVector.add(p,PVector.mult(dirUp,200));

		  _calibXformFlag = true;
		  
		  // calc the inverse
		  _invBaseXform = _baseXform.get();
		  _invBaseXform.invert();

		  // calc the matrix only for normals, no translation
		  _baseXformNormal = _baseXform.get();
		  _baseXformNormal.transpose();
		  _baseXformNormal.invert();

		  
		  _baseXform.mult(_planeP,_planeP);
		  _baseXform.mult(_planeX,_planeX);
		  _baseXform.mult(_planeY,_planeY);

		  return true;
  }

  public void getPlane(PVector p,PVector pX,PVector pY)
  {
	p.set(_planeP);
	pX.set(_planeX);
	pY.set(_planeY);
  }

  public void draw(PGraphics g)
  {
	g.pushStyle();
	g.strokeWeight(1);

	//////////////////////////////////////////////
	// min max area
	//g.pushStyle();
	g.noFill();
        g.stroke(255);
	g.ellipse(0,0,_maxActionRadius*2,_maxActionRadius*2);
	g.ellipse(0,0,_minActionRadius*2,_minActionRadius*2);

	//////////////////////////////////////////////
	// calibration
	if(_scanPosList != null && _scanPosList.size() >= 3)
	{
	  PVector p1 = _scanPosList.get(0);
	  PVector p2 = _scanPosList.get(1);
	  PVector p3 = _scanPosList.get(2);
	  
	  g.strokeWeight(3);
	  g.stroke(255, 0, 0, 200);
	  g.point(p1.x,p1.y,p1.z);


	  g.stroke(0, 255, 0, 200);
	  g.point(p2.x,p2.y,p2.z);


	  g.stroke(0, 0, 255, 200);
	  g.point(p3.x,p3.y,p3.z);
	}

	if(_calibXformFlag)
	{
	  g.strokeWeight(1);

//	  g.stroke(255, 0, 0, 100);
//	  g.line(_pCalibCenter.x,_pCalibCenter.y,_pCalibCenter.z,
//			 _pCalibX.x,_pCalibX.y,_pCalibX.z);
//
//	  g.stroke(0, 255, 0, 100);
//	  g.line(_pCalibCenter.x,_pCalibCenter.y,_pCalibCenter.z,
//			 _pCalibY.x,_pCalibY.y,_pCalibY.z);
//
//	  g.stroke(0, 0, 255, 100);
//	  g.line(_pCalibCenter.x,_pCalibCenter.y,_pCalibCenter.z,
//			 _pCalibZ.x,_pCalibZ.y,_pCalibZ.z);

	  g.pushMatrix();
	  g.applyMatrix(_baseXform);
	  	// draw coordsys
		Utils.drawCoordSys(g,100);
		g.stroke(150, 0, 0, 200);
		Utils.drawPlane(g,
						new PVector(0,0,0),new PVector(1,0,0),new PVector(0,1,0),
						2000, 21);
		
		//////////////////////////////////////////////
		// draw the scene
		drawScene(g);

		drawCurPoly(g);
	  g.popMatrix();

	}
	else
	{
	  //////////////////////////////////////////////
	  // draw the scene
	  drawScene(g);

	  drawCurPoly(g);
	}

	g.popStyle();  

	//////////////////////////////////////////////
	// draw the chain
	_kinematicSolver.draw(g);

  }

  public void draw2d(PGraphics g)
  {
	//////////////////////////////////////////////
	// info viz	
	if((_vizFlags & ROBO_VIZ_INFO) > 0 && _servoVizList != null )
	{
	  g.pushMatrix();
	  g.translate(60,60);
	  for(int i=0;i < _servoVizList.length;i++)
	  {
		_servoVizList[i].draw(g,50);
		g.translate(50 * 2 + 20,0);
	  }  
	  g.popMatrix();
	}
  }

  protected void drawCurPoly(PGraphics g)
  {
	if(_curOutPoly != null)
	{
	  PVector vec;
	  ListIterator<PVector> itr;
	  itr = _curOutPoly.listIterator();
	  
	  g.stroke(255,255,0);
	  g.strokeWeight(2);
	  g.beginShape();
	  while(itr.hasNext()) 
	  {
		vec = itr.next();
		g.vertex(vec.x,vec.y,vec.z);
	  }
	  g.endShape();
	}
  }

  public void drawScene(PGraphics g)
  {
	if(_polygonList == null)
	  return;

	ListIterator<PVector> itr;
	PVector vec;
	ArrayList<PVector> poly;
	ListIterator<ArrayList<PVector>> polyItr = _polygonList.listIterator();

	g.pushStyle();
	g.noFill();
	g.stroke(0,233,0);
	g.strokeWeight(.5f);

    while(polyItr.hasNext()) 
	{
	  poly = polyItr.next();
	  itr = poly.listIterator();
	  g.beginShape();
	  //g.beginShape(PConstants.POINTS);
	  while(itr.hasNext()) 
	  {
		vec = itr.next();
		g.vertex(vec.x,vec.y,vec.z);
	  }
	  g.endShape();
	}

	// draw min max
	g.stroke(255,255,255,100);
	g.beginShape();
		g.vertex(_minVec.x,_minVec.y,_minVec.z);
		g.vertex(_minVec.x,_maxVec.y,_minVec.z);
		g.vertex(_maxVec.x,_maxVec.y,_minVec.z);
		g.vertex(_maxVec.x,_minVec.y,_minVec.z);
	g.endShape(PConstants.CLOSE);

	g.beginShape();
		g.vertex(_minVec.x,_minVec.y,_maxVec.z);
		g.vertex(_minVec.x,_maxVec.y,_maxVec.z);
		g.vertex(_maxVec.x,_maxVec.y,_maxVec.z);
		g.vertex(_maxVec.x,_minVec.y,_maxVec.z);
	g.endShape(PConstants.CLOSE);

	g.beginShape(PConstants.LINES);
		g.vertex(_minVec.x,_minVec.y,_minVec.z);
		g.vertex(_minVec.x,_minVec.y,_maxVec.z);

		g.vertex(_minVec.x,_maxVec.y,_minVec.z);
		g.vertex(_minVec.x,_maxVec.y,_maxVec.z);

		g.vertex(_maxVec.x,_maxVec.y,_minVec.z);
		g.vertex(_maxVec.x,_maxVec.y,_maxVec.z);

		g.vertex(_maxVec.x,_minVec.y,_minVec.z);
		g.vertex(_maxVec.x,_minVec.y,_maxVec.z);
	g.endShape(PConstants.CLOSE);

	g.popStyle();

  }

  public int vizFlags() { return _vizFlags; }
  public void setVizFlags(int vizFlags) { _vizFlags = vizFlags; } 
  
  public void setDrawingSpeed(float drawing, float pos, float posSet,float posDown,float posUp)
  {
	_polygonDrawSpeed = drawing;		
	_positioningSpeed = pos;		
	_polygonDrawSpeed = posSet;	
	_positioningDownSpeed	= posDown;
	_positioningUpSpeed	= posUp;
  }

  protected void resetPolygonTimer()
  {
	_curPolygonStartTime = System.currentTimeMillis();

	_curPolygonNormTime = 0;
	_curPolygonTime = 0;
  }

  protected void resetPolygonTimer(float length,float mmPerSec)
  {
	_curPolygonStartTime = 0;
	_curPolygonNormTime = 0;
	_curPolygonTime = 0;

	_curPolygonTotalTime = (int)(length / mmPerSec * 1000.0f +.5f);
	//System.out.println("length: " + length + ", " + "mmPerSec: " + mmPerSec + ", " + "_curPolygonTotalTime: " + _curPolygonTotalTime);
  }

  protected void updatePolygonTimer()
  {
	_curPolygonTime = System.currentTimeMillis() - _curPolygonStartTime;
	_curPolygonNormTime = 1.0f / _curPolygonTotalTime * _curPolygonTime;
  }

  public boolean enable()
  {
	synchronized(_lock)
	{
	  return _enable;
	}
  }

  public void setEnable(boolean enable)
  {
	synchronized(_lock)
	{
	  _enable = enable; 
	}
  }

  public boolean isDrawing()
  {
	synchronized(_lock)
	{
	  if(_robotState >= ROBO_STATE_DRAW_PRE_POSITION && _robotState <= ROBO_STATE_START)
		return true;
	}
	return false;
  }


}
